#include "cylinder.h"
#include "Fitting/gauss/CylinderFitter.h"
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Dense>

namespace  ELEMENT {
    Cylinder::Cylinder():Element()
    {
        type = ElementType::Cylinder;
    }

    bool Cylinder::fit(const std::vector<Point> &points)
    {
        Fitting::FittingBase* fb = new Gauss::CylinderFitter();
        auto err = fb->Fitting(points, &cylinder);
        delete fb;

        if(err<0) {
            return false;
        }

        // 计算长度
        double lmax=0, lmin=0;
        for(auto &p: points) {
            double l = (p-cylinder.center).dot(cylinder.orient);
            if(l>lmax)lmax=l;
            if(l<lmin)lmin=l;
        }
        len = lmax-lmin;

        int pnum=100;
        vertices.resize(pnum*2);
        index.resize(pnum*3*2);
        auto first = (points[0]-cylinder.center).cross(cylinder.orient).normalized();
        first*=cylinder.r;
        auto ax = Eigen::AngleAxisd(M_PI*2/pnum, cylinder.orient).toRotationMatrix();
        vertices[0] = cylinder.center+first+cylinder.orient*lmax;
        vertices[1] = cylinder.center+first+cylinder.orient*lmin;

        for(int i=0;i<pnum;++i) {
            for(int j=0;j<3;++j) {
                index[i*6+j]=(i*2+j)%(pnum*2);
                index[i*6+j+3]=(i*2+3-j+pnum)%(pnum*2);
            }
            if(i){
                first = ax*first;
                vertices[i*2] = cylinder.center+first+cylinder.orient*lmax;
                vertices[i*2+1] = cylinder.center+first+cylinder.orient*lmin;
            }
        }

        return true;
    }

    void Cylinder::Draw(QOpenGLShaderProgram &shader)
    {
        shader.bind();
        shader.setUniformValue("userColor",QVector4D(0.2f, 0.2f, 0.0f, 1.0f));
        QOpenGLVertexArrayObject::Binder bind(&VAO);
        glDrawElements(GL_TRIANGLES, index.size(), GL_UNSIGNED_INT, 0);
    }

    QString Cylinder::Info()
    {
        QString info("类型：圆柱\n长度：%8\n半径：%1\n轴心：%2 %3 %4\n法向：%5 %6 %7");
        return info.arg(cylinder.r)
                .arg(cylinder.center.x()).arg(cylinder.center.y()).arg(cylinder.center.z())
                .arg(cylinder.orient.x()).arg(cylinder.orient.y()).arg(cylinder.orient.z())
                .arg(len);
    }

}
